﻿#ifndef KINCOSERVO_H
#define KINCOSERVO_H

#include <QObject>
#include <QMutexLocker>
#include <QMutex>
#include <QJsonObject>
#include "Can/canopenbus.h"

class KincoRoParams
{
public:
    uint8_t CanDeviceNo;   //两个伺服的站号
    int8_t RO_WorkMode_int8 ;//工作模式
    uint16_t RO_StateWord_u16 ;    //状态字
    int32_t RO_RealPosition_int32 ;     //实际位置
    float RO_RealSpeed_int32 ;    //实际速度
    float RO_RealCurrent_int16 ;  //实际电流
    uint16_t RO_WarnStateWord_u16 ;    //警告状态字
};

enum KINCOOBJECTINDEX{
    /************************常用字典索引(最后表示该参数的数据类型)*****************/
    RO_WorkMode_int8 = 0x606100,//工作模式
    RO_StateWord_u16 = 0x604100,    //状态字
    RO_RealPosition_int32 = 0x606300,     //实际位置
    RO_RealSpeed_int32 = 0x606C00,    //实际速度  步进int16
    RO_RealCurrent_int16 = 0x607800,  //实际电流
    RO_WarnStateWord_u16 = 0x268000,    //警告状态字  步进无
    RO_PositiveLimit_int32 = 0x20101B,    //正限位位置
    RO_NagetiveLimit_int32 = 0x20101C,    //负限位位置
    RO_RealVoltage_u16 = 0x607900,    //步进电机，实际总线电压

    RW_WorkMode_int8 = 0x606000,     //工作模式
    RW_ControlWord_u16 = 0x604000,  //控制字
    RW_TargetPosition_int32 = 0x607A00,   //目标位置
    RW_TrapeSpeed_u32 = 0x608100,   //梯形速度
    RW_TrapeAcc_u32 = 0x608300, //梯形加速度
    RW_TrapeDec_u32 = 0x608400, //梯形减速度
    RW_TargetSpeed_int32 = 0x60FF00,  //目标速度
    RW_TargetTorque_int16 = 0x607100, //目标扭矩 步进无
    RW_EncoderReset_u8 = 0x269000, //编码器数据复位
    RW_OriginMode_u8 = 0x609800, //原点模式
    RW_CanRate_u8 = 0x2F8100, //can波特率
    RW_CanHeartTime_u16 = 0x101700, //can心跳报文产生时间
    RW_ServoBreak_u8 = 0x641017, //电机附件，1-带抱闸，0-不带抱闸，默认0
    RW_DeviceReboot_uint16 = 0x2FFF00,//驱动器重启
    RW_Save_uint8 = 0x101001    //存储所有参数
};

enum KINCOWORKMODE{
    /***********************工作模式**********************************/
    WorkMode_Pulse = -4,        //脉冲模式
    WorkMode_ImmediateSpeed = -3,//立即速度模式
    WorkMode_Location = 1,//位置模式
    WorkMode_Speed = 3,//速度模式
    WorkMode_Torque = 4,//力矩模式
    WorkMode_FindOrigin = 6,//找原点模式
    WorkMode_Runin = 7//插补模式
};

enum KINCOCANRATE{
    /***********************can波特率********************************/
    CanRate_1M = 100, //1M
    CanRate_500K = 50, //500k
    CanRate_250K = 25, //250k
    CanRate_125k = 12, //125k
    CanRate_50k = 5 //50k
};

enum KINCOSTATUSWORD{
    /**********************状态字位定义*********************************/
    Status_PowerReady = 0x0001,//bit0：准备上电
    Status_PowerOn = 0x0002,//bit1：已上电
    Status_Enable = 0x0004,//bit2：使能
    Status_Fault = 0x0008,//bit3：故障
    Status_FbdOutVoltage = 0x0010,//bit4：禁止输出电压
    Status_QuickStop = 0x0020,//bit5：快速停止
    Status_PowerForbid = 0x0040,//bit6：上电禁止
    Status_Warning = 0x0080,//bit7：警告
    Status_RemoteControl = 0x0200,//bit9：远程控制
    Status_TargetPositionArrive = 0x0400,//bit10：目标位置到
    Status_InterLimitEnable = 0x0800,//bit11：内部限位激活
    Status_PulseResponse = 0x1000,//bit12：脉冲响应
    Status_OriginError = 0x2000,//bit13：跟随误差/原点错误
    Status_MotorExcitation = 0x4000,//bit14：找到电机励磁
    Status_OriginFind = 0x8000//bit15：原点找到
};

enum KINCOCONTROLWORD{
    /**************************控制字定义*********************************/
     ControlWord_MotorPowerOff  = 0x06 , //电机断电
     ControlWord_MotorPowerOn  = 0x0F , //电机上电
     ControlWord_ShutDown  = 0x0B , //快速停止，负载停止，电压断开
     ControlWord_AbsoluteLocationStep1  = 0x2F , //进入绝对定位指令1
     ControlWord_AbsoluteLocationStep2  = 0x3F , //进入绝对定位指令2
     ControlWord_RelativeLocationStep1  = 0x4F , //进入相对定位指令1
     ControlWord_RelativeLocationStep2  = 0x5F , //进入相对定位指令2
     ControlWord_Follow  = 0x103F , //根据目标位置变化立即绝对定位
     ControlWord_OringLocationStep1  = 0x0F , //进入原点定位指令1
     ControlWord_OringLocationStep2  = 0x1F , //进入原点定位指令2
     ControlWord_ClearInterFault  = 0x86 //清除内部故障
};

enum KINCOMNT{
    /**************************MNT状态定义********************************/
    MNTCommand_OpenNode = 0x01,    //开启远程节点
    MNTCommand_CloseNode = 0x02,    //关闭远程节点
    MNTCommand_PreOperation = 0x80,    //进入预操作状态
    MNTCommand_ResetNode = 0x81,    //复位节点
    MNTCommand_Reset_Communication= 0x82,    //复位通讯

    MNTStatus_BootUp = 0x00,   //启动状态
    MNTStatus_Stoped = 0x04,    //停止状态
    MNTStatus_Operation = 0x05,    //运行状态
    MNTStatus_PreOperation = 0x7F    //预操作状态
};

enum KINCOMOTORTYPE{
    ServoType = 0,     //伺服电机类型定义
    StepType = 1      //步进电机类型定义
};

class KincoServo : public QObject
{
    Q_OBJECT
public:
    //const static int32_t KincoEncoderResolvingRatio_Servo = 65536;    //步科伺服编码器分辨率，用于相关参数计算
    //const static int32_t KincoEncoderResolvingRatio_Step = 60000;    //步科步进编码器分辨率，用于相关参数计算
    const static int16_t COF_Speed = 17895; //伺服电机速度转换系数  (512*编码器分辨率)/1875
    const static int16_t COF_Speed_Step = 16384; //步进电机速度转换系数  (512*编码器分辨率)/1875
    const static int16_t COF_Acc = 1073;  //加速度转换系数  (65536*编码器分辨率)/4000000
    const static int16_t COF_Acc_Step = 983;  //加速度转换系数  (65536*编码器分辨率)/4000000
    constexpr static float COf_Current = 0.445; //电流转换系数  1Arms = [2048/(6510.03/1.414)]DEC

    const static int32_t Rotate_p90 = 1179648; //旋转电机转动正90度编码器位置，旋转盘齿轮比1：18,65536*18
    const static int32_t Rotate_n90 = -1179648;


    /***************************其他参数*********************************/
    const  static uint16_t Comman_Reboot = 0xAA55;    //驱动器重启
    const static uint8_t Command_Save = 0x01;   //保存参数

    uint8_t nodeid {0};
    uint8_t type {0};

    int8_t mRO_WorkMode {0}; //有效工作模式
    uint16_t mRo_StatusWord {0}; //状态字
    int32_t mRo_RealPosition {0};    //实际位置
    float mRo_RealSpeed {0}; //实际速度
    float mRo_RealCurrent {0};   //实际电流
    uint16_t mRo_WarnWord {0};   //警告字
    int32_t mRO_PositiveLimit {0};    //正限位位置
    int32_t mRO_NagetiveLimit  {0};     //负限位位置

    uint16_t mRO_RealVoltage {0};    //步进电机，实际总线电压

    int8_t mRW_WorkMode {0};      //工作模式
    uint16_t mRW_ControlWord {0};   //控制字
    int32_t mRW_TargetPosition {0};    //目标位置
    uint32_t mRW_TrapeSpeed {0};    //梯形速度
    uint32_t mRW_TrapeAcc {0};  //梯形加速度
    uint32_t mRW_TrapeDec {0};  //梯形减速度
    int32_t mRW_TargetSpeed {0};   //目标速度
    uint16_t mRW_TargetTorque {0};  //目标扭矩 步进无
    uint8_t mRW_EncoderReset {0};  //编码器数据复位
    uint8_t mRW_OriginMode {0};  //原点模式
    uint8_t mRW_CanRate {0};  //can波特率
    uint16_t mRW_CanHeartTime {0};  //can心跳报文产生时间
    uint8_t mRW_ServoBreak {0};  //电机附件，1-带抱闸，0-不带抱闸，默认0
    uint16_t mRW_DeviceReboot {0}; //驱动器重启
    uint8_t mRW_Save {0};     //存储所有参数
    uint8_t mHeartServo1 {0};    //心跳

    uint8_t mServoLocation {0};    //五个电机的位置状态
public:
    explicit KincoServo(CanOpenBus *canbus, uint8_t nodeid, uint8_t motortype, QObject *parent = nullptr);

    bool SetServoParam(int32_t objectindex, int32_t value);
    void analysisSDOParam(const CanopenSDOFrame&);

    bool SetRwWorkMode_SDO(int8_t data);
    bool GetRwWorkMode_SDO();
    bool SetRwControlWord_SDO(uint16_t data);
    bool GetRwControlWord_SDO();
    bool SetRwTargetPosition_SDO(int32_t data);
    bool GetRwTargetPosition_SDO();
    bool SetRwTrapeSpeed_SDO(uint32_t data);
    bool GetRwTrapeSpeed_SDO();
    bool SetRwTrapeAcc_SDO(uint32_t data);
    bool GetRwTrapeAcc_SDO();
    bool SetRwTrapeDec_SDO(uint32_t data);
    bool GetRwTrapeDec_SDO();
    bool SetRwTargetSpeed_SDO(int32_t data);
    bool GetRwTargetSpeed_SDO();
    bool SetRwTargetTorque_SDO(int16_t data);
    bool GetRwTargetTorque_SDO();
    bool SetRwEncoderReset_SDO(uint8_t data);
    bool GetRwEncoderReset_SDO();
    bool SetRwCanRate(uint8_t data);
    bool GetRwCanRate();
    bool SetRwCanHeartTime(uint16_t data);
    bool GetRwCanHeartTime();
    bool SetRwServoBreak(uint8_t data);
    bool GetRwServoBreak();
    bool SetRwOriginMode_SDO(int8_t data);
    bool GetRwOriginMode_SDO();

    bool GetRoWorkMode_SDO();
    bool GetRoStateWord_SDO();
    bool GetRoRealPosition_SDO();
    bool GetRoRealSpeed_SDO();
    bool GetRoRealCurrent_SDO();
    bool GetRoWarnStateWord_SDO();
    bool GetRoPositiveLimit();
    bool GetRoNagetiveLimit();
    bool GetRoRealVoltage();

    bool SetMNTStatus(uint8_t cmd);
    // 速度单位换算
    float SpeedDec2Rpm(int32_t dec);
    int32_t SpeedRpm2Dec(int rpm);
    // 加减速度单位换算
    float AccDec2Rps(uint32_t);
    uint32_t AccRps2Dec(uint32_t);
    //电流单位换算
    float CurrentDec2Arms(uint32_t);
    uint32_t CurrentArms2Dec(uint32_t);

    void ReadRoParams();
    void ReadRwParams();

    bool ArriveTargetPosition_PDO(int32_t targetposition,uint16_t controlword);
    bool ArriveOriginPosition_PDO();

private:
    void Frame_SDOProcess(QCanBusFrame frameData);
signals:

private slots:
    void slot_receivedFrame(QCanBusFrame frame);
private:
    CanOpenBus *myCanbus {nullptr};

};

#endif // KINCOSERVO_H
